// #include "rclcpp/rclcpp.hpp"
// #include "serial_driver/serial_driver.hpp"
// #include <string>

// unsigned char testSend3=0x07;
// float pos[5] = {0};
// int joint[5] = {0}; 
 
// const unsigned char ender[2] = {0x0d, 0x0a};
// const unsigned char header[2] = {0x55, 0xaa};

// union sendData
// {
//     short d;
//     unsigned char data[2];
// };
// union sendData angle[7];

// union receiveData
// {
//     short d;
//     unsigned char data[2];
// } leftVelNow, rightVelNow, angleNow;

// class Serial_Node : public rclcpp::Node
// {
// public:
//   Serial_Node() : Node("serial_node_cpp")
//   {
//     // 等设备准备好再初始化
//     // std::this_thread::sleep_for(std::chrono::milliseconds(500));
 
//     // 串口设备名（根据实际设备调整）
//     const std::string device_name = "/dev/ttyACM0";
 
//     RCLCPP_INFO(this->get_logger(), "Serial port Node Open!");
//     // 创建串口配置对象
//     // 波特率115200；不开启流控制；无奇偶效验；停止位1。
//     drivers::serial_driver::SerialPortConfig config(
//         19200,
//         drivers::serial_driver::FlowControl::NONE,
//         drivers::serial_driver::Parity::NONE,
//         drivers::serial_driver::StopBits::ONE);
    
//     // 初始化串口
//     try
//     {
//       io_context_ = std::make_shared<drivers::common::IoContext>(1);
//       // 初始化 serial_driver_
//       serial_driver_ = std::make_shared<drivers::serial_driver::SerialDriver>(*io_context_);
//       serial_driver_->init_port(device_name, config);
//       serial_driver_->port()->open();
      
//       RCLCPP_INFO(this->get_logger(), "Serial port initialized successfully");
//       RCLCPP_INFO(this->get_logger(), "Using device: %s", serial_driver_->port().get()->device_name().c_str());
//       RCLCPP_INFO(this->get_logger(), "Baud_rate: %d", config.get_baud_rate());
//     }
//     catch (const std::exception &ex)
//     {
//       RCLCPP_ERROR(this->get_logger(), "Failed to initialize serial port: %s", ex.what());
//       return;
//     }
 
//         // 设置发送定时器
//     transmit_timer_ = this->create_wall_timer(
//         std::chrono::milliseconds(5000),
//         std::bind(&Serial_Node::send_message_timer_callback, this));

//     writeSpeed(joint, testSend3);
//     // async_receive_message();   //进入异步接收
//   }
 
// private:
 
//   void send_message_timer_callback()
//   {
//     // 发送一串字符串
//     const std::string transmitted_message = "Hello ROS2 SDUT VinciRobot!\n";
//     transmit_data_buffer = std::vector<uint8_t>(transmitted_message.begin(), transmitted_message.end());
//     // std::vector<uint8_t> hex_data = {0x48, 0x65, 0x6C, 0x6C, 0x6F}; // "Hello" in ASCII
//     auto port = serial_driver_->port();
 
//     try
//     {
//       size_t bytes_transmit_size = port->send(transmit_data_buffer);
//       RCLCPP_INFO(this->get_logger(), "Transmitted: %s (%ld bytes)", transmitted_message.c_str(), bytes_transmit_size);
//     }
//     catch(const std::exception &ex)
//     {
//       RCLCPP_ERROR(this->get_logger(), "Error Transmiting from serial port:%s",ex.what());
//     }
//   }
 
//   void writeSpeed(int *p, unsigned char ctrlFlag)
//   {
//       unsigned char buf[20] = {0};
//       int i, length = 0;
  
//       // 处理输入的速度数据并填充 angle 数组
//       for (int i = 0; i < 7; i++)
//       {
//           angle[i].d = p[i];
//       }
  
//       // 填充消息头
//       for (i = 0; i < 2; i++)
//           buf[i] = header[i];
  
//       length = 13;  // 定义消息长度
//       buf[2] = length;  // 填充长度字段
  
//       // 填充数据部分
//       for (i = 0; i < 2; i++)
//       {
//           buf[i + 3] = angle[0].data[i];
//           buf[i + 5] = angle[1].data[i];
//           buf[i + 7] = angle[2].data[i];
//           buf[i + 9] = angle[3].data[i];
//           buf[i + 11] = angle[4].data[i];
//           buf[i + 13] = angle[5].data[i];
//       }
  
//       // 填充控制标志
//       buf[3 + length - 1] = ctrlFlag;
  
//       // 计算并填充校验和
//       buf[3 + length] = getCrc8(buf, 3 + length);
  
//       // 填充消息尾部
//       buf[3 + length + 1] = ender[0];
//       buf[3 + length + 2] = ender[1];
  
//       // 将 buf 数组转换为 std::vector<uint8_t>
//       std::vector<uint8_t> data_to_send(buf, buf + 20);
  
//       try
//       {
//           // 使用 serial_driver 发送数据
//           size_t bytes_transmit_size = serial_driver_->port()->send(data_to_send); // 发送数据
//           RCLCPP_INFO(this->get_logger(), "Sent data: %ld bytes", bytes_transmit_size);  // 打印发送的字节数
//       }
//       catch (const std::exception &ex)
//       {
//           RCLCPP_ERROR(this->get_logger(), "Error sending data: %s", ex.what());  // 错误处理
//       }
//   }
  
  

//   unsigned char getCrc8(unsigned char *ptr, unsigned short len)
//     {
//         unsigned char crc;
//         unsigned char i;
//         crc = 0;
//         while (len--)
//         {
//             crc ^= *ptr++;
//             for (i = 0; i < 8; i++)
//             {
//                 if (crc & 0x01)
//                     crc = (crc >> 1) ^ 0x8C;
//                 else
//                     crc >>= 1;
//             }
//         }
//         return crc;
//     }

 
//   std::shared_ptr<drivers::serial_driver::SerialDriver> serial_driver_;
//   std::shared_ptr<drivers::common::IoContext> io_context_;
//   rclcpp::TimerBase::SharedPtr transmit_timer_;
//   std::vector<uint8_t> transmit_data_buffer = std::vector<uint8_t>(1024); // 发送缓冲区
//   std::vector<uint8_t> receive_data_buffer = std::vector<uint8_t>(1024);  // 接收缓冲区
// };
 
// int main(int argc, char **argv)
// {
//   rclcpp::init(argc, argv);
 
//   auto node = std::make_shared<Serial_Node>();
 
//   rclcpp::spin(node);
 
//   rclcpp::shutdown();
//   return 0;
// }


#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/joint_state.hpp"  // ROS2 的消息类型

unsigned char testSend3 = 0x07;
float pos[5] = {0};
int joint[5] = {0};

// PublicNode 类
class PublicNode : public rclcpp::Node
{
public:
    PublicNode() : Node("public_node")
    {
        // 创建订阅者：订阅 "/joint_states" 话题
        subscription_ = this->create_subscription<sensor_msgs::msg::JointState>(
            "/joint_states", 10, std::bind(&PublicNode::Callback, this, std::placeholders::_1));

    }

private:
    rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;

    // 将 Callback 函数作为 PublicNode 的成员函数
    void Callback(const sensor_msgs::msg::JointState::SharedPtr msg)
    {
        int len = msg->position.size();
        for (int j = 0; j < len && j < 5; j++) {
            pos[j] = msg->position[j];
        }
        if (len >= 5) {
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "pos,%f,%f,%f,%f,%f", pos[0], pos[1], pos[2], pos[3], pos[4]);
            for (int i = 0; i < 5; i++) {
                joint[i] = static_cast<int>(pos[i] * 10000);
            }
            // writeSpeed(joint, testSend3); // 调用自定义的串口发送函数
            RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Data was transmitted.\n");
        }
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);  // 初始化 ROS2
    auto node = std::make_shared<PublicNode>();  // 创建节点对象
    rclcpp::spin(node);  // 启动事件循环
    rclcpp::shutdown();  // 关闭 ROS2
    return 0;
}
